import os
import cv2
from src.data.structures import BinocularImagePair
from src.infrastructure.logger import setupLogger
from src.algorithms.calibration import ZhangCalibration
from src.algorithms.rectification import BinocularRectifier
from src.utils.utils import autoLoadImagePairs
from src.utils.visualize import visualizeRectification
from src.utils.tune_sgbm_gui import tuneSgbmGui


def main():
    logger = setupLogger("test_calibration")

    # 1. 加载图像对
    folder = os.path.join(os.path.dirname(__file__), "images")
    logger.info(f"加载图像对: {folder}")
    pairs = autoLoadImagePairs(
        folder, left_pattern="*left*", right_pattern="*right*", ext="png"
    )
    if not pairs:
        logger.error("未加载到任何图像对, 请检查路径和文件名模式")
        return

    # 2. 相机标定
    logger.info("开始相机标定...")
    calib = ZhangCalibration(board_size=(9, 9), square_size=10.0)
    calib_data = calib.binocularCalibration(pairs, visualize=True)

    logger.info(f"左相机内参:\n{calib_data.camera_matrix_left}")
    logger.info(f"右相机内参:\n{calib_data.camera_matrix_right}")
    logger.info(f"旋转矩阵:\n{calib_data.rotation_matrix}")
    logger.info(f"平移向量:\n{calib_data.translation_vector}")
    logger.info(f"重投影误差: {calib_data.reprojection_error}")

    # 3. 图像校正
    logger.info("开始图像校正...")
    rectifier = BinocularRectifier(calib_data)
    rectified_pair = rectifier.rectifyPair(pairs[1], True)
    visualizeRectification(pairs[1], rectified_pair, delay=0)

    # 4. 调参计算视差
    logger.info("开始视差计算...")
    best_params = tuneSgbmGui(rectified_pair.left_image, rectified_pair.right_image)
    logger.info(f"最优参数：{best_params}")


if __name__ == "__main__":
    main()
